#include "Quaternion.h"

double Quaternion::delta = 0.00005;

void Quaternion::CreateFromAxisAngle(float X, float Y, float Z, float degree) {
	float angle = (degree / 180.0f) * PI;
	float result = sinf(angle / 2.0f);
	w = cosf(angle / 2.0f);
	x = X * result;
	y = Y * result;
	z = Z * result;
}

void Quaternion::CreateMatrix(float *pMatrix)
{
	if(!pMatrix) return;
	//First row
	pMatrix[ 0] = 1.0f - 2.0f * ( y * y + z * z );
	pMatrix[ 1] = 2.0f * ( x * y - w * z );
	pMatrix[ 2] = 2.0f * ( x * z + w * y );
	pMatrix[ 3] = 0.0f;

	//Second row
	pMatrix[ 4] = 2.0f * ( x * y + w * z );  
	pMatrix[ 5] = 1.0f - 2.0f * ( x * x + z * z );  
	pMatrix[ 6] = 2.0f * ( y * z - w * x );  
	pMatrix[ 7] = 0.0f;  

	// Third row
	pMatrix[ 8] = 2.0f * ( x * z - w * y );  
	pMatrix[ 9] = 2.0f * ( y * z + w * x );  
	pMatrix[10] = 1.0f - 2.0f * ( x * x + y * y );  
	pMatrix[11] = 0.0f;  

	// Fourth row
	pMatrix[12] = 0;  
	pMatrix[13] = 0;  
	pMatrix[14] = 0;  
	pMatrix[15] = 1.0f;
}

void Quaternion::CreateFromMatrix(float *pMatrix) {
	float tr, s;
	float q[4];
	int i, j, k;
	int nxt[3] = {1, 2, 0};
	tr = pMatrix[0] + pMatrix[5] + pMatrix[10];
	if(tr > 0.0f) {
		s = sqrtf(tr + 1.0f);
		w = s / 2.0f;
		s = 0.5f / s;
		x = (pMatrix[9] - pMatrix[6]) * s;
		y = (pMatrix[2] - pMatrix[8]) * s;
		z = (pMatrix[4] - pMatrix[1]) * s;
	} else {
		i = 0;
		if(pMatrix[5] > pMatrix[0]) i = 1;
		if(pMatrix[10] > pMatrix[i*4+i]) i = 2;
		j = nxt[i];
		k = nxt[j];
		s = sqrtf((pMatrix[i*4+i] - 
			(pMatrix[j*4+j] + pMatrix[k*4+k])) + 1.0f);
		q[i] = s * 0.5f;
		if(s != 0.0f) s = 0.5f / s;
		q[3] = (pMatrix[k*4+j] - pMatrix[j*4+k]) * s;
		q[j] = (pMatrix[j*4+i] + pMatrix[i*4+j]) * s;
		q[k] = (pMatrix[k*4+i] - pMatrix[i*4+k]) * s;
		x = q[0];
		y = q[1];
		z = q[2];
		w = q[3];
	}
}

void Quaternion::QuatSlerp(Quaternion from, Quaternion to, float t) {
	float to1[4];
	double omega, cosom, sinom, scale0, scale1;
	cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
	if(cosom < 0.0f) {
		cosom = -cosom;
		to1[0] = -to.x;
		to1[1] = -to.y;
		to1[2] = -to.z;
		to1[3] = -to.w;
	} else {
		to1[0] = to.x;
		to1[1] = to.y;
		to1[2] = to.z;
		to1[3] = to.w;
	}
	if((1.0f - cosom) > delta) {
		omega = cos(cosom);
		sinom = sin(omega);
		scale0 = sin((1.0f - t) * omega) / sinom;
		scale1 = sin(t * omega) / sinom;
	} else {
		scale0 = 1.0f - t;
		scale1 = t;
	}
	x = (float)(scale0 * from.x + scale1 * to1[0]);
	y = (float)(scale0 * from.y + scale1 * to1[1]);
	z = (float)(scale0 * from.z + scale1 * to1[2]);
	w = (float)(scale0 * from.w + scale1 * to1[3]);
}

void Quaternion::normalize() {
	float d = sqrtf(x*x + y*y + z*z + w*w);
	x /= d;
	y /= d;
	z /= d;
	w /= d;
}

float Quaternion::ToAngleByXAxis() {
	float s, c;
	if (abs(w) > 1.0f || abs(x) > 1.0f) normalize();
	return 2.0f * atan2f(x, w);
}